#include "swarm_avoid_client/swarm_avoid.hpp" 
#include "swarm_avoid_client/global_functions.hpp"

#include <asio.hpp>
#include <chrono>
#include <thread>
#include <string>
#include <nlohmann/json.hpp>

// 使用 nlohmann/json 命名空间
using json = nlohmann::json;
using namespace std::chrono_literals; // Enables usage of chrono literals

AirplaneController::AirplaneController(const std::string& namespace_prefix)
    : Node("airplane_controller", namespace_prefix), // 将命名空间传递给 Node
      namespace_prefix_(namespace_prefix),
      controller_(),
      takeoff_signal_received_(false),
      initial_position_(),
      initial_position_recorded_(false),
      flag_init_position(false),
      init_flag(false),
      flag_arrive(false),
      first_detected(false),
      takeoff_in_progress_(true),
      target_takeoff_height_(10.0),
      current_state_(),
      local_pose_(),
      current_velocity_(),
      last_request_(),
      wait_times(0),
      goal_received_(false){
    

    this->declare_parameter<std::string>("namespace", "/drone" );
    this->declare_parameter<int>("vehicle_id", 1 );
    this->declare_parameter<int>("vehicle_number", 4 );
    

    namespaces = this->get_parameter("namespace").as_string();
    vehcile_id_ = this->get_parameter("vehicle_id").as_int();
    vehcile_num_ = this->get_parameter("vehicle_number").as_int();
    namespaces = namespaces + "_" + std::to_string(vehcile_id_);


    // 声明参数
    // this->declare_parameter<std::string>("parameters.namesspace", "drone_12" );
    // this->declare_parameter<int>("parameters.vehicle_id", 1 );
    // this->declare_parameter<int>("parameters.vehicle_num", 4 );
    // // 获取参数
    // namespaces = this->get_parameter("parameters.namesspace").as_string();
    // vehcile_id_ = this->get_parameter("parameters.vehicle_id").as_int();
    // vehcile_num_ = this->get_parameter("parameters.vehicle_num").as_int();

    RCLCPP_INFO(this->get_logger(),"parameters.namesspace:%s",namespaces.c_str());
    RCLCPP_INFO(this->get_logger(),"parameters.vehicle_id:%d",vehcile_id_);
    RCLCPP_INFO(this->get_logger(),"parameters.vehicle_num:%d",vehcile_num_);
    std::vector<std::string> other_vehciles;
    other_vehciles.clear();
    
    for(int i = 0; i <vehcile_num_&&i!=vehcile_id_; i++  ){
        std::string  vehcile = namespace_prefix +"_" + std::to_string(i);
        other_vehciles.push_back(vehcile);
    }
    for(int i = vehcile_id_+1; i <vehcile_num_&&i!=vehcile_id_; i++  ){
        std::string  vehcile = namespace_prefix +"_"+ std::to_string(i);
        other_vehciles.push_back(vehcile);
    }

    namespace_prefix_ = namespaces;
    // tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    // tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
    initializeServiceClients();
    initializeServiceServer();
    initializeSubscribers(other_vehciles);
    initializePublishers();
    // startMulticastReceiver("224.0.1.4", 7880); // 启动组播接收

    RCLCPP_INFO(this->get_logger(), "Namespace prefix set to: %s", namespace_prefix_.c_str());
    initializeTimer();

    std::string number_str;
    bool found_number = false;
    for(char ch:namespace_prefix_){
        if(std::isdigit(ch)){
            number_str.push_back(ch);
            found_number = true;
        }else if(found_number){
            break;
        }
    }
    if(!number_str.empty())
    {
        id_ = std::stoi(number_str);
    }
}

void AirplaneController::initializeSubscribers(const std::vector<std::string>& other_airplanes) {
    state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
        namespace_prefix_ + "/state", 1, std::bind(&AirplaneController::stateCallback, this, std::placeholders::_1));

    local_pos_sb = this->create_subscription<sensor_msgs::msg::NavSatFix>(
        namespace_prefix_ + "/global_position/global", rclcpp::QoS(10).best_effort(), std::bind(&AirplaneController::localPosCallback, this, std::placeholders::_1));
    RCLCPP_INFO(this->get_logger(), "Initial: %s", (namespace_prefix_+"/global_position/global").c_str());

    takeoff_sync_sub_ = this->create_subscription<std_msgs::msg::Bool>(
        namespace_prefix_ + "/cmd/takeoff_sync", 1, std::bind(&AirplaneController::takeoffSyncCallback, this, std::placeholders::_1));

    for (const auto& airplane : other_airplanes) {
        auto sub = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            airplane + "/global_position/global", rclcpp::QoS(20).best_effort(),
            [this, airplane](const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
                otherAirplanePosCallback(airplane, msg);
            });
        RCLCPP_INFO(this->get_logger(), "Initial: %s", (airplane+"/global_position/global").c_str());
        other_airplane_pos_subscribers_.emplace_back(sub);
    }
}

void AirplaneController::initializePublishers() {
    // mavros_velocity_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
    //     namespace_prefix_ + "/setpoint_velocity/cmd_vel", 1);
    target_motion_pub_ = this->create_publisher<yolo_detect_message::msg::MoveInterface>(
        "/uav_swarms" + namespace_prefix_ + "/move", 1);
        // /setpoint_trajectory/desired
}

void AirplaneController::initializeServiceClients() {
    set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(
        namespace_prefix_ + "/set_mode");
    arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>(
        namespace_prefix_ + "/cmd/arming");
    takeoff_client_ = this->create_client<mavros_msgs::srv::CommandBool>(
        namespace_prefix_ + "/cmd/takeoff");
}

void AirplaneController::initializeServiceServer() {
    set_goal_service_ = this->create_service<interfaces::srv::SetGoal>(
        namespace_prefix_ + "/set_goal",
        std::bind(&AirplaneController::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
    
    if (set_goal_service_) {
        RCLCPP_INFO(this->get_logger(), "Service '%s/set_goal' successfully registered.", namespace_prefix_.c_str());
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to register service '%s/set_goal'.", namespace_prefix_.c_str());
    }
}

void AirplaneController::initializeTimer() {
    timer_ = this->create_wall_timer(100ms, std::bind(&AirplaneController::timerLoop, this));
}

/**
 * @brief 从 JSON 字符串中解码位置信息
 * @param json_data 输入的 JSON 字符串
 * @return 解码后的位置信息（Location 结构体）
 * @throws std::invalid_argument 如果 JSON 数据无效或缺少必要字段
 */
udpData AirplaneController::decode_location(const std::string& json_data) {
    // 解析 JSON 数据
    json parsed_data;
    try {
        parsed_data = json::parse(json_data);
    } catch (const json::parse_error& e) {
        throw std::invalid_argument("无效的 JSON 数据: " + std::string(e.what()));
    }

    // 检查是否存在必要的字段
    if (!parsed_data.contains("lat") || !parsed_data.contains("lon") || !parsed_data.contains("alt")|| !parsed_data.contains("id")|| !parsed_data.contains("radius")) {
        throw std::invalid_argument("JSON 数据中缺少必要的字段");
    }

    // 提取字段值并填充到结构体中
    udpData location;
    location.lat = parsed_data["lat"];
    location.lon = parsed_data["lon"];
    // location.name = parsed_data["name"];
    location.id = parsed_data["id"];
    location.alt = parsed_data["alt"];
    location.radius = parsed_data["radius"];

    return location;
}

// // 重载 startMulticastReceiver 函数，支持读取组播数据
// void startMulticastReceiver1(const std::string& multicast_address, int multicast_port) {
//     multicast_address_ = multicast_address;
//     multicast_port_ = multicast_port;

//     io_context_ = std::make_shared<asio::io_context>();
//     multicast_socket_ = std::make_shared<asio::ip::udp::socket>(*io_context_);

//     // 绑定到本地端点
//     asio::ip::udp::endpoint listen_endpoint(asio::ip::udp::v4(), multicast_port_);
//     multicast_socket_->open(listen_endpoint.protocol());
//     multicast_socket_->set_option(asio::ip::udp::socket::reuse_address(true));
//     multicast_socket_->bind(listen_endpoint);

//     // 加入组播组
//     multicast_socket_->set_option(asio::ip::multicast::join_group(
//         asio::ip::address::from_string(multicast_address_)));

//     // 准备接收缓冲区
//     auto buffer = std::make_shared<std::vector<char>>(1024); // 使用足够大的缓冲区接收 JSON 数据
//     auto sender_endpoint = std::make_shared<asio::ip::udp::endpoint>();

//     // 异步接收数据
//     multicast_socket_->async_receive_from(
//         asio::buffer(*buffer), *sender_endpoint,
//         [this, buffer, sender_endpoint](const asio::error_code& error, std::size_t bytes_received) {
//             if (!error) {
//                 std::string json_data(buffer->data(), bytes_received); // 将接收到的数据转换为字符串
//                 try {
//                     udpData data = decode_location(json_data); // 解析 JSON 数据
//                     Obstacle received_obstacle;
//                     int index = 0; // 假设索引为 0，可以根据需要修改
//                     handleMulticastMessage(data, received_obstacle, index);
//                     controller_.add_obstacle(received_obstacle, index);

//                     RCLCPP_INFO(this->get_logger(), "Received obstacle from %s:%d: lat=%.2f, lon=%.2f, alt=%.2f, radius=%.2f, name=%s",
//                                 sender_endpoint->address().to_string().c_str(), sender_endpoint->port(),
//                                 data.lat, data.lon, data.alt, data.radius, data.name.c_str());
//                 } catch (const std::exception& e) {
//                     RCLCPP_ERROR(this->get_logger(), "Failed to parse JSON data: %s", e.what());
//                 }
//             } else {
//                 RCLCPP_ERROR(this->get_logger(), "Failed to receive multicast message: %s", error.message().c_str());
//             }
//             startMulticastReceiver(multicast_address_, multicast_port_); // 继续接收下一条消息
//         });

//     // 在单独线程中运行 IO 上下文
//     std::thread([this]() { io_context_->run(); }).detach();
// }

// 重载 startMulticastReceiver 函数，支持指定 IP 和端口
void AirplaneController::startMulticastReceiver(const std::string& ip, int port) {
    multicast_address_ = ip;
    multicast_port_ = port;

    io_context_ = std::make_shared<asio::io_context>();
    multicast_socket_ = std::make_shared<asio::ip::udp::socket>(*io_context_);

    asio::ip::udp::endpoint listen_endpoint(asio::ip::udp::v4(), multicast_port_);
    multicast_socket_->open(listen_endpoint.protocol());
    multicast_socket_->set_option(asio::ip::udp::socket::reuse_address(true));
    multicast_socket_->bind(listen_endpoint);
    multicast_socket_->set_option(asio::ip::multicast::join_group(asio::ip::address::from_string(multicast_address_)));

    auto buffer = std::make_shared<std::vector<char>>(1024); // 使用足够大的缓冲区接收 JSON 数据
    auto sender_endpoint = std::make_shared<asio::ip::udp::endpoint>();

    multicast_socket_->async_receive_from(
        asio::buffer(*buffer), *sender_endpoint,
        [this, buffer, sender_endpoint](const asio::error_code& error, std::size_t bytes_received) {
            if (!error) {
                std::string json_data(buffer->data(), bytes_received); // 将接收到的数据转换为字符串
                try {
                    udpData data = decode_location(json_data); // 解析 JSON 数据
                    Obstacle received_obstacle;
                    int index = 0; // 假设索引为 0，可以根据需要修改
                    handleMulticastMessage(data, received_obstacle, index);
                    controller_.add_obstacle(received_obstacle, index);

                    RCLCPP_INFO(this->get_logger(), "Received obstacle from %s:%d: lat=%.2f, lon=%.2f, alt=%.2f, radius=%.2f, name=%s",
                                sender_endpoint->address().to_string().c_str(), sender_endpoint->port(),
                                data.lat, data.lon, data.alt, data.radius, data.name.c_str());
                } catch (const std::exception& e) {
                    RCLCPP_ERROR(this->get_logger(), "Failed to parse JSON data: %s", e.what());
                }
            } else {
                RCLCPP_ERROR(this->get_logger(), "Failed to receive multicast message: %s", error.message().c_str());
            }
            startMulticastReceiver(multicast_address_, multicast_port_); // 继续接收下一条消息
        });
    std::thread([this]() { io_context_->run(); }).detach();
}

void AirplaneController::handleMulticastMessage(const udpData data, Obstacle& obstacle, int &index) {
    double x, y, z;
    wgs84_to_enu(data.lat, data.lon, data.alt, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    x, y, z);
    index = data.id;
    obstacle = Obstacle(Vector3d(x, y, z), data.radius);
}

void AirplaneController::timerLoop() {
    if (!goal_received_) return;

    // auto offboard_request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    // offboard_request->custom_mode = "OFFBOARD";
    // set_mode_client_->async_send_request(offboard_request);

    // // 检查无人机状态，若未解锁则调用 armDrone() 进行解锁和垂直起飞
    // if (takeoff_in_progress_) {
    //     // Set the goal position in controller

    //         // 控制无人机垂直上升到10米高度
    //         if (local_pose_.pose.position.z < target_takeoff_height_ - 0.1) { // 留一个容忍范围
                
    //             // auto takeoff_request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
    //             // takeoff_request->value = true;
    //             // takeoff_client_->async_send_request(takeoff_request);
    //             setVelocity(Vector3d(0., 0, 1)); // 垂直上升
    //             // RCLCPP_INFO(this->get_logger(), "Takeoff to %f.",local_pose_.pose.position.z);
    //         } else {
    //             // 达到目标高度，停止上升
    //             setVelocity(Vector3d(0.0, 0.0, 0.0)); // 停止垂直运动
    //             takeoff_in_progress_ = false; // 完成垂直起飞
    //             takeoff_signal_received_ = true;
    //             RCLCPP_INFO(this->get_logger(), "Takeoff to 5 meters complete, starting main control...");
    //         }
    //     return; // 在垂直起飞完成之前不执行其他控制逻辑
    // }else{

    double distance_to_goal = sqrt(pow((controller_.goal(0) - current_position_(0)),2)+pow((controller_.goal(1) - current_position_(1)),2));
    RCLCPP_INFO(this->get_logger(), "distance_to_goal %f.",distance_to_goal);
    if(distance_to_goal > 0.5)
    {
        controller_.set_positions(current_position_, other_positions_);
        Vector3d speed = controller_.compute_force();
        setVelocity(speed);
    }
    else
    {
        setVelocity(Vector3d(0,0,0.0));
        goal_received_ = false;
    }
}

void AirplaneController::armDrone() {
    if (!current_state_.connected) {
        RCLCPP_WARN(this->get_logger(), "Waiting for FCU connection...");
        return;
    }

    if (!current_state_.armed) {
        auto arm_request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
        arm_request->value = true;
        arming_client_->async_send_request(arm_request);
    }

    auto offboard_request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    offboard_request->custom_mode = "OFFBOARD";
    set_mode_client_->async_send_request(offboard_request);

    takeoff_in_progress_ = true;
}

void AirplaneController::setVelocity(const Vector3d& velocity) {
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex_);
    geometry_msgs::msg::Twist cmd_vel;
    cmd_vel.linear.x = velocity.x();
    cmd_vel.linear.y = velocity.y();
    cmd_vel.linear.z = velocity.z();

    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = 0.0;

    target_motion_.is_vel = true;
    target_motion_.is_body = false;
    target_motion_.vel = cmd_vel;
    target_motion_.is_gps = false;

    target_motion_pub_->publish(target_motion_);
}

bool AirplaneController::add_obstacle(const Obstacle& obstacle, int index)
{

    controller_.add_obstacle(obstacle, index);
    return true;
}

void AirplaneController::takeoffSyncCallback(const std_msgs::msg::Bool::SharedPtr msg) {
    //  = msg->data;
}

void AirplaneController::stateCallback(const mavros_msgs::msg::State::SharedPtr msg) {
    current_state_ = *msg;
}

void AirplaneController::localPosCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
    // If not recorded, store the takeoff position
    if (!initial_position_recorded_ ) {
        initial_position_ = *msg;
        initial_position_recorded_ = true;
    }
    else
    {
        auto lat = msg->latitude;
        auto lon = msg->longitude;
        auto alt = msg->altitude;

        double north, east, down;

        wgs84_to_enu(lat, lon, alt, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    north, east, down);

        // Update current position in global coordinates using Eigen::Vector3d
        current_position_ = Vector3d(north, east, down);
    }
}


void AirplaneController::otherAirplanePosCallback(const std::string& airplane_id, const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
    // Convert other drone's position to Eigen::Vector3d
    

    if(initial_position_recorded_)
    {
        sensor_msgs::msg::NavSatFix target_lla = *msg;

        auto lat = msg->latitude;
        auto lon = msg->longitude;
        auto alt = msg->altitude;

        double north, east, down;

        wgs84_to_enu(lat, lon, alt, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    north, east, down);

        // Update current position in global coordinates using Eigen::Vector3d
        Vector3d other_position = Vector3d(north, east, down);

        // RCLCPP_INFO(this->get_logger(), "position recorded for airplane: %s, position x:%f", airplane_id.c_str(), other_position.x());
        // RCLCPP_INFO(this->get_logger(), "position recorded for airplane: %s, position y:%f", airplane_id.c_str(), other_position.y());
        // RCLCPP_INFO(this->get_logger(), "position recorded for airplane: %s, position z:%f", airplane_id.c_str(), other_position.z());

        // Store it in the map
        other_positions_[airplane_id] = other_position;
    }

}

void AirplaneController::setGoalCallback(const std::shared_ptr<rmw_request_id_t> request_header, 
                                          const std::shared_ptr<interfaces::srv::SetGoal::Request> request, 
                                          const std::shared_ptr<interfaces::srv::SetGoal::Response> response) {
    // Update the goal coordinates
    double lat,lon,alt;
    lon = static_cast<double>(request->goal_x);
    lat = static_cast<double>(request->goal_y);
    alt = static_cast<double>(request->goal_z);

    // 121.346347
    // 25.1256243
    // 540
    RCLCPP_INFO(this->get_logger(), "recieved goal: (lon %.2f, lat %.2f, alt %.2f)", lon,lat,alt);
    wgs84_to_enu(lat, lon, alt, 
                initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                goal_x_, goal_y_, goal_z_);

    goal_x_ += id_*4;

    RCLCPP_INFO(this->get_logger(), "initial_position_: (%.2f, %.2f, %.2f)", initial_position_.latitude, initial_position_.longitude, initial_position_.altitude);
    RCLCPP_INFO(this->get_logger(), "Goal received: (%.2f, %.2f, %.2f)", goal_x_, goal_y_, goal_z_);

    Vector3d goal(goal_x_, goal_y_, goal_z_);
    std::unordered_map<int, Obstacle> obstacles;

    GlobalPosition obstacleCenter1(121.3463809, 25.1230791, 1130);
    // GlobalPosition obstacleCenter2();

    double x,y,z;

    wgs84_to_enu(obstacleCenter1.latitude, obstacleCenter1.longtitude, obstacleCenter1.altitude, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    x, y, z);

    // obstacles.emplace_back(Obstacle(Vector3d(3.0, 80.0, 0.0), 10.0));  // 添加障碍物
    obstacles[0] = (Obstacle(Vector3d(x, y, z), 80.0));  // 添加障碍物

    GlobalPosition obstacleCenter2(121.250042028, 25.11529103, 1130);
    wgs84_to_enu(obstacleCenter2.latitude, obstacleCenter2.longtitude, obstacleCenter2.altitude, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    x, y, z);
    obstacles[1] = (Obstacle(Vector3d(x, y, z), 100.0));  // 添加障碍物
    GlobalPosition obstacleCenter3(121.252481783, 25.1125620837, 1130);
    wgs84_to_enu(obstacleCenter3.latitude, obstacleCenter3.longtitude, obstacleCenter3.altitude, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    x, y, z);
    obstacles[2] = (Obstacle(Vector3d(x, y, z), 100.0));  // 添加障碍物
    GlobalPosition obstacleCenter4(121.249824966, 25.112479292, 1130);
    wgs84_to_enu(obstacleCenter4.latitude, obstacleCenter4.longtitude, obstacleCenter4.altitude, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    x, y, z);
    obstacles[3] = (Obstacle(Vector3d(x, y, z), 100.0));  // 添加障碍物
    GlobalPosition obstacleCenter5(121.25266027, 25.1150360606, 1130);
    wgs84_to_enu(obstacleCenter5.latitude, obstacleCenter5.longtitude, obstacleCenter5.altitude, 
                    initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
                    x, y, z);
    obstacles[4] = (Obstacle(Vector3d(x, y, z), 100.0));  // 添加障碍物

    // wgs84_to_enu(obstacleCenter2.latitude, obstacleCenter2.longtitude, obstacleCenter2.altitude, 
    //                 initial_position_.latitude, initial_position_.longitude, initial_position_.altitude,
    //                 x, y, z);

    // obstacles.emplace_back(Obstacle(Vector3d(x, y, z), 10.0));  // 添加障碍物


    controller_.set_positions(current_position_ , other_positions_);
    controller_.set_goal(goal);
    controller_.set_obstacles(obstacles);

    // Set flag to indicate the goal is received
    goal_received_ = true;

    // Provide success response
    response->success = true;
    
}

// geometry_msgs::msg::PointStamped AirplaneController::TransformToBaseLink(
//     const geometry_msgs::msg::PointStamped local_point, const geometry_msgs::msg::PointStamped other_point)
// {
//     std::string current_frame = "base_link";
//     try{
//         geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_->lookupTransform(
//             current_frame, target_point.header.frame_id, rclcpp::Time(0));

//             geometry_msgs::msg::PointStamped transformed_point;
//             tf2::doTransform(local_point, transformed_point, transform_stamped);

//             return transformed_point;
//     }
//     catch (const tf2::TransformException &ex){
//         RCLCPP_WARN(this->get_logger(),"could not transform from %s to %s",
//         target_point.header.frame_id.c_str(), current_frame.c_str)
//     }
// }

